/*******************************************************************************
 * Copyright (C) 2013 Jeremy Brodt and Kevin Gillespie
 * Description: Global definitions and configurations
 *         $Id: car.h 27 2014-04-13 22:57:20Z brodtj1212@gmail.com $
 *******************************************************************************
 */

#ifndef _CAR_H_
#define _CAR_H_

#define NUM_WHEELS  2

#define LEFT        0
#define RIGHT       1
#define CENTER      2

#define FORWARD     0
#define REVERSE     1

// Modes
typedef enum {
  FIXED_VALUE = 0,
  FIXED_SPEED = 1,
  TURNING = 2,
  COASTING = 3,
  PAUSE = 4
} op_mode_t;

#define SPEED_LOG_SIZE    60
#define DIST_LOG_SIZE     50

#define TRIP_DEST_INIT    100

#define NUM_OBJ_SENSORS   3
#define MIN_DISTANCE      40

// d = 42 mm, c = pi * d = 131.947 mm
// TODO - will need to correct odometer for fraction
#if 0
#define WHEEL_CIRCUM_10MM_LEFT    1345  // 10 * mm
#define WHEEL_CIRCUM_10MM_RIGHT   1342  // 10 * mm
#define WHEEL_CIRCUM_MMS4_LEFT    2152  // 134.5 mm * 16
#define WHEEL_CIRCUM_MMS4_RIGHT   2147  // 134.2 mm * 16
#endif
#if 0
#define WHEEL_CIRCUM_10MM_LEFT    1345  // 10 * mm
#define WHEEL_CIRCUM_10MM_RIGHT   1340  // 10 * mm
#define WHEEL_CIRCUM_MMS4_LEFT    2152  // 134.5 mm * 16
#define WHEEL_CIRCUM_MMS4_RIGHT   2144  // 134.0 mm * 16
#endif
#if 0
#define WHEEL_CIRCUM_10MM_LEFT    1345  // 10 * mm
#define WHEEL_CIRCUM_10MM_RIGHT   1334  // 10 * mm
#define WHEEL_CIRCUM_MMS4_LEFT    2152  // 134.5 mm * 16
#define WHEEL_CIRCUM_MMS4_RIGHT   2134  // 133.4 mm * 16
#endif
#if 0
#define WHEEL_CIRCUM_10MM_LEFT    1341  // 10 * mm
#define WHEEL_CIRCUM_10MM_RIGHT   1330  // 10 * mm
#define WHEEL_CIRCUM_MMS4_LEFT    2146  // mm * 16
#define WHEEL_CIRCUM_MMS4_RIGHT   2128  // mm * 16
#endif
#if 1
#define WHEEL_CIRCUM_10MM_LEFT    1338  // 10 * mm
#define WHEEL_CIRCUM_10MM_RIGHT   1327  // 10 * mm
#define WHEEL_CIRCUM_MMS4_LEFT    2141  // mm * 16
#define WHEEL_CIRCUM_MMS4_RIGHT   2123  // mm * 16
#endif

#define START_VALUE_LEFT    0x2C
#define START_VALUE_RIGHT   0x29

#define WHEELBASE_MM    107   // mm
//#define WHEELBASE_MM    100   // mm
//#define WHEELBASE_MM    90    // mm
//#define WHEELBASE_MM    85    // mm
//#define WHEELBASE_MM    80    // mm

#if (WHEELBASE_MM == 107)
#define DIST_ROTATE_90_DEG    84
#define DIST_ROTATE_180_DEG   168
#define DIST_ROTATE_360_DEG   336
#endif
#define VALUE_ROTATE  0x25

#define TURN_RADIUS   547
//#define TURN_RADIUS   500
//#define TURN_RADIUS   300

#if (TURN_RADIUS == 500)

#if (WHEELBASE_MM == 107)
#define ARC_INSIDE  785   // mm
#define ARC_OUTSIDE 953   // mm
#define in_whl_spd(setpoint)   ((setpoint * 40) / 49)
#elif (WHEELBASE_MM == 100)
#define ARC_INSIDE  785   // mm
#define ARC_OUTSIDE 937   // mm
#define in_whl_spd(setpoint)   ((setpoint * 10) / 12)
#elif (WHEELBASE_MM == 90)
#define ARC_INSIDE  785   // mm
#define ARC_OUTSIDE 927   // mm
#define in_whl_spd(setpoint)   ((setpoint * 10) / 12)
#elif (WHEELBASE_MM == 85)
#define ARC_INSIDE  785   // mm
#define ARC_OUTSIDE 918   // mm
#define in_whl_spd(setpoint)   ((setpoint * 30) / 35)
#elif (WHEELBASE_MM == 80)
#define ARC_INSIDE  785   // mm
#define ARC_OUTSIDE 911   // mm
#define in_whl_spd(setpoint)   ((setpoint * 30) / 35)
#else
#error Unsupported WHEELBASE_MM
#endif

#elif (TURN_RADIUS == 300)

#if (WHEELBASE_MM == 107)
#define ARC_INSIDE  471   // mm
#define ARC_OUTSIDE 639   // mm
#define in_whl_spd(setpoint)   ((setpoint * 25) / 34)
#elif (WHEELBASE_MM == 97)
#define ARC_INSIDE  471   // mm
#define ARC_OUTSIDE 624   // mmop
#define in_whl_spd(setpoint)   ((setpoint * 30) / 40)
#elif (WHEELBASE_MM == 85)
#define ARC_INSIDE  471   // mm
#define ARC_OUTSIDE 605   // mm
#define in_whl_spd(setpoint)   ((setpoint * 30) / 38)
#elif (WHEELBASE_MM == 80)
#define ARC_INSIDE  471   // mm
#define ARC_OUTSIDE 597   // mm
#define in_whl_spd(setpoint)   ((setpoint * 30) / 38)
#else
#error Unsupported WHEELBASE_MM
#endif

#elif (TURN_RADIUS == 547)
#if(WHEELBASE_MM == 107)
#define ARC_INSIDE 859
#define ARC_OUTSIDE 1027
#define in_whl_spd(setpoint) ((setpoint * 10) / 12)
#endif
#else
#error Unsupported turning radius
#endif

#define TURN_LAT_DIST   (TURN_RADIUS + ((WHEELBASE_MM + 1) / 2))

void print_i32(int32_t value);

#define calc_odometer_value(wheel, value) ((value * hundred_mm_per_cnt[wheel]) / 100)

#define DIST_BETWEEN_SENSORS        15  // mm
#define wall_dist(width)            ((width - DIST_BETWEEN_SENSORS) / 2)
#define wall_width(distl, distr)    (distl + distr + DIST_BETWEEN_SENSORS)

#endif /* _CAR_H_ */
